// --------------------------------------------------------------------------------------------------------------------
// <copyright file="TurretScannerAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;

    /// <summary>
    /// Implements turret scanning behavior, filtering Kinect frames.
    /// </summary>
    [DataContract]
    public class TurretScannerAgent : ConsumerProducerAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>, DepthCameraAgentMessage<KinectDepthCameraParameters>>,
        ISubscriptionReceiver<JointMessage>, // need this to know turret rotation speed and determine stable positions
        IControlReceiver<TurretScanInitiationMessage>
    {
        /// <summary>
        /// Name of the heartbeat agent
        /// </summary>
        [DataMember(Name = "HeartbeatAgent")]
        private string heartbeatAgentName = default(string);

        /// <summary>
        /// Size of a single step during turret scan.
        /// </summary>
        [DataMember(Name = "TurretSweepStepsInRadians")]
        private double? turretSweepStepsInRadians;

        /// <summary>
        /// Minimal angle in radians from where to start sweeps
        /// </summary>
        [DataMember(Name = "TurretSweepRangeInRadiansStart")]
        private double? turretSweepRangeInRadiansStart;

        /// <summary>
        /// Maximum angle in radians where to stop sweeps
        /// </summary>
        [DataMember(Name = "TurretSweepRangeInRadiansEnd")]
        private double? turretSweepRangeInRadiansEnd;

        /// <summary>
        /// Turret rotation speed below which we'll consider Kinect as 'stable' and re-transmit
        /// </summary>
        [DataMember(Name = "ConsiderTurretStableIfRotatingSlowerThanThisInRadsPerSecond")]
        private double? considerTurretStableIfRotatingSlowerThanThis;

        /// <summary>
        /// Delay between initiating sweeps
        /// </summary>
        [DataMember(Name = "TurretHoldPositionInMilliseconds")]
        private int? turretHoldPositionInMilliseconds;

        /// <summary>
        /// Position between TurretSweepPositionStart and TurretSweepPositionEnd where turret is rested while driving forward
        /// </summary>
        [DataMember(Name = "TurretDefaultHeading")]
        private double? turretDefaultHeading;

        /// <summary>
        /// Desired Turret rotation speed in radians per second
        /// </summary>
        [DataMember(Name = "DesiredTurretPanRotationSpeed")]
        private double desiredTurretPanRotationSpeed;

        /// <summary>
        /// Turret pan message forwarder.
        /// </summary>
        private IForwarder<SetServoSequenceMessage> panForwarder;

        /// <summary>
        /// Turret scanner utility.
        /// </summary>
        private TurretScanner scanner;

        /// <summary>
        /// Initializes a new instance of the <see cref="TurretScannerAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="heartbeatAgentName">Name of the heartbeat agent</param>
        /// <param name="kinectAgentName">Name of the Kinect message producer agent</param>
        /// <param name="turretSweepStepsInRadians">Turret Sweep Step In Radians</param>
        /// <param name="turretHoldPositionInMilliseconds">Turret Hold Position In Milliseconds</param>
        /// <param name="turretSweepRangeInRadiansStart">Turret Sweep Range In Radians Start</param>
        /// <param name="turretSweepRangeInRadiansEnd">Turret Sweep Range In Radians End</param>
        /// <param name="turretDefaultHeading">Turret Default Heading</param>
        /// <param name="considerTurretStableIfRotatingSlowerThanThis">Consider Turret Stable If Rotating Slower Than This</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="desiredTurretPanRotationSpeed">Desired turret pan rotation speed</param>
        public TurretScannerAgent(
            string name,
            string heartbeatAgentName,
            string kinectAgentName,
            double turretSweepStepsInRadians,
            int turretHoldPositionInMilliseconds,
            double turretSweepRangeInRadiansStart,
            double turretSweepRangeInRadiansEnd,
            double turretDefaultHeading,
            double considerTurretStableIfRotatingSlowerThanThis,
            string robotConfigFolder,
            double desiredTurretPanRotationSpeed)
            : base(name, kinectAgentName)
        {
            this.heartbeatAgentName = heartbeatAgentName;
            this.turretSweepStepsInRadians = turretSweepStepsInRadians;
            this.turretHoldPositionInMilliseconds = turretHoldPositionInMilliseconds;
            this.turretSweepRangeInRadiansStart = turretSweepRangeInRadiansStart;
            this.turretSweepRangeInRadiansEnd = turretSweepRangeInRadiansEnd;
            this.turretDefaultHeading = turretDefaultHeading;
            this.considerTurretStableIfRotatingSlowerThanThis = considerTurretStableIfRotatingSlowerThanThis;
            this.desiredTurretPanRotationSpeed = desiredTurretPanRotationSpeed;
        }

        /// <summary>
        /// Called on agent initialization
        /// </summary>
        /// <param name="locator">The agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            if (this.heartbeatAgentName == default(string) ||
                this.turretSweepRangeInRadiansEnd.Value - this.turretSweepRangeInRadiansStart.Value <= 0 ||
                this.turretSweepStepsInRadians > ((this.turretSweepRangeInRadiansEnd.Value - this.turretSweepRangeInRadiansStart.Value)) ||
                this.turretDefaultHeading.Value > this.turretSweepRangeInRadiansEnd.Value ||
                this.turretDefaultHeading.Value < this.turretSweepRangeInRadiansStart.Value ||
                !this.turretHoldPositionInMilliseconds.HasValue ||
                this.turretHoldPositionInMilliseconds.Value < 1 ||
                !this.considerTurretStableIfRotatingSlowerThanThis.HasValue ||
                this.desiredTurretPanRotationSpeed == 0)
            {
                throw new ArgumentException(string.Format("At least one required initialization parameter is missing for {0}.", this.GetType().FullName));
            }

            this.scanner = new TurretScanner(
                this.considerTurretStableIfRotatingSlowerThanThis.Value,
                this.turretDefaultHeading.Value,
                this.desiredTurretPanRotationSpeed,
                this.turretSweepStepsInRadians.Value,
                this.turretSweepRangeInRadiansStart.Value,
                this.turretSweepRangeInRadiansEnd.Value,
                this.turretHoldPositionInMilliseconds.Value);

            IAgentHost heartbeatAgent = locator.GetAgent(this.heartbeatAgentName);
            this.panForwarder = heartbeatAgent.GetControlPort<SetServoSequenceMessage>();
            heartbeatAgent.Subscribe((ISubscriptionReceiver<JointMessage>)this);

            base.Initialize(locator);
        }

        /// <summary>
        /// Handle Kinect agent message, filter out frames that are received in a non-stable mode (i.e. when robot is moving)
        /// </summary>
        /// <param name="message">Message with Kinect frame</param>
        public override void Receive(DepthCameraAgentMessage<KinectDepthCameraParameters> message)
        {
            if (this.scanner.IsStable)
            {
                this.Publisher.Post(message);
            }
        }

        /// <summary>
        /// We need to process joint message to determine slow speed at which we'll retransmit stable Kinect frames 
        /// </summary>
        /// <param name="jointMsg">Turret Joint state</param>
        public void Receive(JointMessage jointMsg)
        {
            const int PanIndex = 1;
            this.scanner.UpdateTurret(
                currentTurretPanRotationSpeed: jointMsg.State[PanIndex].CurrentVelocityInRadiansPerSecond,
                turretCurrentPanAngle: (float)jointMsg.State[PanIndex].PositionInRadians);
        }

        /// <summary>
        /// Initiate a turret scan.
        /// </summary>
        /// <param name="message">Control message triggering turret scan.</param>
        public async void ReceiveControl(TurretScanInitiationMessage message)
        {
            TraceOut.Info(TraceContexts.Common, "Turret scan in progress...");
            await this.scanner.InitiateTurretScan((heading, duration) =>
            {
                this.panForwarder.Post(new SetServoSequenceMessage((float)heading, duration, message));
            });
            TraceOut.Info(TraceContexts.Common, "Turret scan complete.");
        }
    }
}
